package es.uji.viselab.robot.links;

import es.uji.viselab.kinematics.DKinematic;
import es.uji.viselab.math.Joints;
import es.uji.viselab.math.Matrix4;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RealRobot;
import es.uji.viselab.robot.RobotCatalog;
import es.uji.viselab.robot.RobotException;
import es.uji.viselab.robot.links.fanuc.RobotCommunicationFanucARCMate120;

public class RobotLinkFanucARCMate120 extends RealRobot {

	RobotCommunicationFanucARCMate120 fanuc;
	 private DKinematic dKinematic;

	public void connect() throws RobotException {
		super.connect();
		fanuc.Connect();
		updateEndEffectorPositionMatrix();
	}

	public void connect(String ip, int port) throws RobotException {
		super.connect(ip, port);
		fanuc.Connect();
		updateEndEffectorPositionMatrix();
		notifyPositionChange(this);

	}
	private void updateEndEffectorPositionMatrix() throws RobotException {
		Joints j = fanuc.getJoints();
		double[] v = j.getArray();
		pose = dKinematic.getDirectKinematicMatrix(v);
	}
	
	public RobotLinkFanucARCMate120(RobotCatalog model) {
		super(model);
		fanuc = new RobotCommunicationFanucARCMate120();
		dKinematic = new DKinematic(robotModel);
	
	// Stablish the pose of the robot based on its current position:
		/*try {
			getJointsV(); // this method get the joints values, but more important: updates the pose
		} catch (RobotException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}*/
	}

	@Override
	public boolean isRobotConnected() {
		// TODO Auto-generated method stub
		return false;
	}

	@Override
	public boolean isConnected() {
		// TODO Auto-generated method stub
		return false;
	}

	@Override
	public String ping() throws RobotException {
		return fanuc.Ping();
	}

	@Override
	public void disconnect() throws RobotException {
		// TODO Auto-generated method stub

	}

	@Override
	public Joints getJoints() throws RobotException {
		return fanuc.getJoints();
	}
	
	
	public double[] getJointsV() throws RobotException{
		Joints j = fanuc.getJoints();
		double[] v = j.getArray();
		pose = dKinematic.getDirectKinematicMatrix(v);
		return v;
		
	}

	@Override
	public void move(Joints j) throws RobotException {
		fanuc.Move(j);
		double[] v = j.getArray();
		pose = dKinematic.getDirectKinematicMatrix(v);
		notifyPositionChange(this);
	}

	

	@Override
	public String getName() {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public boolean isVirtual() {
		return false;
	}

	@Override
	public void move(double[] v) throws RobotException {
		RobotToolLocation rtl= fanuc.getXYZWPR();
		PointXYZ location = new PointXYZ();
		location.setX(rtl.getLocation().getX()+v[0]*1000);
		location.setY(rtl.getLocation().getY()+v[1]*1000);
		location.setZ(rtl.getLocation().getZ()+v[2]*1000);
		//location.setZ(rtl.getLocation().getZ());
		
		PointXYZ orientation = new PointXYZ();
		orientation.setX(rtl.getOrientation().getX());
		orientation.setY(rtl.getOrientation().getY());
		orientation.setZ(rtl.getOrientation().getZ()+v[5]);//*2*Math.PI/260);
		
		rtl.setLocation(location);
		rtl.setOrientation(orientation);
        fanuc.Move(rtl);
    //    try {
	//		Thread.sleep(1000);
	//	} catch (InterruptedException e) {
	//		// TODO Auto-generated catch block
	//		e.printStackTrace();
	//	}
        
        //updateLink
        updateEndEffectorPositionMatrix();
        notifyPositionChange(this);
	}

	private void sleep(double d) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void setPosition(double[] v) throws RobotException {
		RobotToolLocation rtl = new RobotToolLocation(v[0] * 1000, v[1] * 1000, v[2] * 1000,
				v[3] * 1000, v[4] * 1000, v[5] * 1000);
		fanuc.Move(rtl);
		updateEndEffectorPositionMatrix();
		notifyPositionChange(this);
	}

}